#include "KalmanFilter.h"
#include <iostream>


KalmanFilter::KalmanFilter(int Nparams,float pError = 0.001,float qError = 0.1,float rError = 0.001)
{
	
	P=cv::Mat(Nparams,Nparams,CV_32F);
	R=cv::Mat(Nparams,Nparams,CV_32F);
	Q=cv::Mat(Nparams,Nparams,CV_32F);
	H=cv::Mat(Nparams,Nparams,CV_32F);
	I=cv::Mat(Nparams,Nparams,CV_32F);
	
	setIdentity(I);
	setIdentity(H);
	
	setIdentity(P,pError);
	setIdentity(Q,0.001);
	setIdentity(R,0.001);
	
	X = cv::Mat::zeros(Nparams,1,CV_32F); 
	Z = cv::Mat::zeros(Nparams,1,CV_32F);
	
}


KalmanFilter::~KalmanFilter(void)
{
}

void KalmanFilter::filter()
{
	cv::Mat At,Ht,y,S,K;
	transpose(A,At);
	transpose(H,Ht);

	X=A*X;	 
	P=A*P*At+Q;
	y=Z-H*X;
    S=H*P*Ht+R;
    K=P*H*S;
    X=X+(K*y);
    P=(I-K*H)*P;
}


void KalmanFilter::drawTrack(cv::Mat &Img,cv::Point z,cv::Point x)
{
	zVector.push_back(z);
	xVector.push_back(x);

	if(xVector.size()>1)
	{
		for(int i = 1; i<xVector.size(); i++)
		{
			line(Img,xVector[i-1],xVector[i],cv::Scalar(0,0,255));

			line(Img,zVector[i-1],zVector[i],cv::Scalar(255,100,100));
		}
	}
}